#include"Utils.h"

const double EPSILON = 1e-10;

/*
 * 在两点间补齐途径点
 * start:起始点坐标
 * end:终点坐标
 * interval:间隔
 * return   补齐后的二维数组
*/
QVector<QVector<double>> interpolateSegment(const QVector<double>& start,const QVector<double>& end,double interval){
    QVector<QVector<double>> points;

    // 解构坐标点
    const double x1 = start[0], y1 = start[1], z1 = start[2];
    const double x2 = end[0], y2 = end[1], z2 = end[2];

    // 计算向量分量
    const double dx = x2 - x1;
    const double dy = y2 - y1;
    const double dz = z2 - z1;

    // 计算线段长度
    const double distance = qSqrt(dx*dx + dy*dy + dz*dz);

    // 处理零长度线段
    if (qAbs(distance) < EPSILON) {
        points.append({x1, y1, z1});
        return points;
    }

    // 单位方向向量
    const double dir_x = dx / distance;
    const double dir_y = dy / distance;
    const double dir_z = dz / distance;

    // 添加起点（带精度处理）
    points.append({x1,y1,z1});

    // 参数化插值
    double t = 0.0;
    while (true) {
        // 计算剩余距离
        const double remaining = distance - t;
        if (remaining < interval - EPSILON) break;

        t += interval;
        const double x = x1 + dir_x * t;
        const double y = y1 + dir_y * t;
        const double z = z1 + dir_z * t;

        points.append({x,y,z});
    }

    // 强制添加终点（精度校验）
    const QVector<double>& last = points.last();
    if (qAbs(last[0]-x2) > 1e-9 ||
        qAbs(last[1]-y2) > 1e-9 ||
        qAbs(last[2]-z2) > 1e-9)
    {
        points.append({x2,y2,z2});
    }

    return points;
}

/*
 * 给定一个二维数组，对每相邻两点之间都进行补齐途径点操作
 * path:路径数组
 * interval:间隔
 * return   补齐后的二维数组
 */
QVector<QVector<double>> interpolatePointsSequence(const QVector<QVector<double>>& path,double interval){
    if (path.size() < 2) {
        return path.isEmpty() ? QVector<QVector<double>>()
                            : QVector<QVector<double>>{path.first()};
    }

    QVector<QVector<double>> fullPath;

    // 首段完整添加
    const auto firstSeg = interpolateSegment(path[0], path[1], interval);
    fullPath.append(firstSeg);

    // 后续线段跳过首点
    for (int i = 1; i < path.size() - 1; ++i) {
        const auto seg = interpolateSegment(path[i], path[i+1], interval);
        if (!seg.isEmpty()) {
            fullPath.reserve(fullPath.size() + seg.size()-1);
            for (auto it = seg.begin()+1; it != seg.end(); ++it) {
                fullPath.append(*it);
            }
        }
    }
    return fullPath;
}

/*
 * 将计算出的最短路径写入数据库
 * path:表示路径的二维数组
 */
void writePath(QVector<QVector<double>>& paths){

    for (auto path :paths){
        QString s;
        for(auto i :path){
            s+=QString::number(i)+',';
        }
        s.chop(1);
        DataBase::getInstance().writeData("fireman1_savepath",s);
    }

}

/*
 * 计算剩余时间和走过的距离
 * map:消防员的路径信息
 * return   返回长度为2的数组，第一个表示剩余时间，第二个表示当前走过的距离
 */
QList<QString> calInfo(QMap<QString,QString>map){
    QList<QString> ans;
    QString time;
    double d1=map["distance"].toDouble()/2;
    QList<double> temp=DataBase::getInstance().getFireManinfo(map["number"].toInt());
    double d2=temp[1]/2;
    double tot=temp[0];
    if(d1!=d2){
        time = QString::number((d2-d1)/tot*0.1);
    }
    else
        time = QString::number(d2);

    ans.append(time);
    ans.append(QString::number(d2-d1));
    return ans;

}
